#include <iostream>
#include <string>
#include <deque>
#include <thread>
#include <Eigen/Eigen>
#include <Eigen/Core>

#include <ros/ros.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/JointState.h>
#include <sensor_msgs/NavSatFix.h>
#include <std_msgs/String.h>
#include <yaml-cpp/yaml.h>
#include <unitree_legged_msgs/HighState.h>


class A1Msg2jointStateMsg
{
public:
    std::string root_dir = ROOT_DIR;
    std::string newBagFile_dir;
    std::string outBagFiles;

    ros::NodeHandle nh;
    ros::Subscriber subLegOdom;
    ros::Subscriber subLidar;
    ros::Subscriber subImu;
    ros::Subscriber subJointState;

    rosbag::Bag newBag;

    std::string A1_leg_odom_Topic = "/state_SDK";
    std::string A1_lidar_Topic = "/points_raw";
    std::string A1_imu_Topic = "/imu_raw";
    std::string A1_jointState_Topic = "/high_state";
    
    std::string leg_odom_Topic = "/leg_odom";
    std::string lidar_Topic = "/velodyne_points";
    std::string imu_Topic = "/xsens/imu/data";
    std::string jointState_Topic = "/joint_states";

    std::string imu_Topic_unitree = "/xsens/imu/data/unitree";

    std::mutex odomLegLock;

    A1Msg2jointStateMsg()
    {
        YAML::Node yaml;
        yaml = YAML::LoadFile(std::string(std::string(ROOT_DIR)) + "config/A1Msg2jointStateConfig.yaml");
        newBagFile_dir = yaml["newBagFile_dir"].as<std::string>();
        outBagFiles = std::string(std::string(ROOT_DIR)) + newBagFile_dir;
        int reflash = system((std::string("exec rm ") + outBagFiles).c_str());

        subLegOdom    = nh.subscribe<nav_msgs::Odometry>(A1_leg_odom_Topic, 1000, &A1Msg2jointStateMsg::legOdometry_handle, this, ros::TransportHints().tcpNoDelay());
        subLidar      = nh.subscribe<sensor_msgs::PointCloud2>(A1_lidar_Topic, 1000, &A1Msg2jointStateMsg::lidar_handle, this, ros::TransportHints().tcpNoDelay());
        subImu        = nh.subscribe<sensor_msgs::Imu>(A1_imu_Topic, 1000, &A1Msg2jointStateMsg::imu_handle, this, ros::TransportHints().tcpNoDelay());
        subJointState = nh.subscribe<unitree_legged_msgs::HighState>(A1_jointState_Topic, 1000, &A1Msg2jointStateMsg::jointState_handle, this,  ros::TransportHints().tcpNoDelay());

        newBag.open(outBagFiles, rosbag::bagmode::Write);
    }

    /**************************************************************
     * @brief 订阅A1_legodom的回调函数
     * 重新赋值时间戳
     * 
     * @param msgIn 
     *************************************************************/
    void legOdometry_handle(const nav_msgs::Odometry::ConstPtr& msgIn)
    {
        std::lock_guard<std::mutex> lock1(odomLegLock);
        nav_msgs::Odometry data = *msgIn;
        // data.header.stamp = ros::Time::now();
        data.header.frame_id = "camera_init";

        newBag.write(leg_odom_Topic, data.header.stamp, data);   
    }

    /**************************************************************
     * @brief 订阅lidar的回调函数
     * 重新赋值时间戳
     * 
     * @param msgIn 
     *************************************************************/
    void lidar_handle(const sensor_msgs::PointCloud2::ConstPtr& msgIn)
    {
        sensor_msgs::PointCloud2 data = *msgIn;
        // data.header.stamp = ros::Time::now();

        newBag.write(lidar_Topic, data.header.stamp, data);
    }

    /**************************************************************
     * @brief 订阅A1_imu的回调函数
     * 重新赋值时间戳
     * 
     * @param msgIn 
     *************************************************************/
    void imu_handle(const sensor_msgs::Imu::ConstPtr& msgIn)
    {
        sensor_msgs::Imu data = *msgIn;
        // data.header.stamp = ros::Time::now();

        newBag.write(imu_Topic, data.header.stamp, data);
    }

    /**************************************************************
     * @brief 订阅A1关节数据的回调函数
     * 重新赋值时间戳
     * 
     * @param msgIn 
     *************************************************************/
    void jointState_handle(const unitree_legged_msgs::HighState::ConstPtr& msgIn)
    {
        unitree_legged_msgs::HighState data = *msgIn;
        // data.stamp = ros::Time::now();

        sensor_msgs::JointState joint_data;

        Eigen::Matrix<double, 3, 4> foot_angle;
        Eigen::Matrix<double, 3, 4> foot_angle_vel;
        Eigen::Matrix<double, 3, 4> foot_angle_tor;

        foot_angle << msgIn->motorState[3].q, msgIn->motorState[0].q, msgIn->motorState[9].q, msgIn->motorState[6].q, 
                      msgIn->motorState[4].q, msgIn->motorState[1].q, msgIn->motorState[10].q, msgIn->motorState[7].q, 
                      msgIn->motorState[5].q, msgIn->motorState[2].q, msgIn->motorState[11].q, msgIn->motorState[8].q;

        foot_angle_vel << msgIn->motorState[3].dq, msgIn->motorState[0].dq, msgIn->motorState[9].dq, msgIn->motorState[6].dq, 
                          msgIn->motorState[4].dq, msgIn->motorState[1].dq, msgIn->motorState[10].dq, msgIn->motorState[7].dq, 
                          msgIn->motorState[5].dq, msgIn->motorState[2].dq, msgIn->motorState[11].dq, msgIn->motorState[8].dq;

        foot_angle_tor << msgIn->motorState[3].tauEst, msgIn->motorState[0].tauEst, msgIn->motorState[9].tauEst, msgIn->motorState[6].tauEst, 
                          msgIn->motorState[4].tauEst, msgIn->motorState[1].tauEst, msgIn->motorState[10].tauEst, msgIn->motorState[7].tauEst, 
                          msgIn->motorState[5].tauEst, msgIn->motorState[2].tauEst, msgIn->motorState[11].tauEst, msgIn->motorState[8].tauEst;

        joint_data.header.stamp = data.stamp;
        joint_data.name.resize(12);
        joint_data.position.resize(12);     // 重置电机角度数据
        joint_data.velocity.resize(12);     // 重置电机角速度数据
        joint_data.effort.resize(12);       // 重置电机力矩数据

        // 左前腿关节电机数据
        joint_data.name[0] = "LF_Joint";
        joint_data.name[1] = "LF_Joint_1";
        joint_data.name[2] = "LF_Joint_2";

        // 右前腿关节电机数据
        joint_data.name[3] = "RF_Joint";
        joint_data.name[4] = "RF_Joint_1";
        joint_data.name[5] = "RF_Joint_2";

        // 左后腿关节电机数据
        joint_data.name[6] = "LB_Joint";
        joint_data.name[7] = "LB_Joint_1";
        joint_data.name[8] = "LB_Joint_2";

        // 右后腿关节电机数据
        joint_data.name[9] = "RB_Joint";
        joint_data.name[10] = "RB_Joint_1";
        joint_data.name[11] = "RB_Joint_2";

        for (int i = 0; i < 12; i++)
        {
            joint_data.position[i] = foot_angle(i%3, i/3);
            joint_data.velocity[i] = foot_angle_vel(i%3, i/3);
            joint_data.effort[i]   = foot_angle_tor(i%3, i/3);
        }

        newBag.write(jointState_Topic, data.stamp, joint_data);
        newBag.write(A1_jointState_Topic, data.stamp, data);

        /************************* 处理unitree数据中的IMU数据 *****************************/
        sensor_msgs::Imu data_unitree;

        data_unitree.header.stamp = data.stamp;
        data_unitree.angular_velocity.x = data.imu.gyroscope[0];
        data_unitree.angular_velocity.y = data.imu.gyroscope[1];
        data_unitree.angular_velocity.z = data.imu.gyroscope[2];
        data_unitree.linear_acceleration.x = data.imu.accelerometer[0];
        data_unitree.linear_acceleration.y = data.imu.accelerometer[1];
        data_unitree.linear_acceleration.z = data.imu.accelerometer[2];
        data_unitree.orientation.x = data.imu.quaternion[0];
        data_unitree.orientation.y = data.imu.quaternion[1];
        data_unitree.orientation.z = data.imu.quaternion[2];
        data_unitree.orientation.w = data.imu.quaternion[3];
        // newBag.write(imu_Topic_unitree, data.stamp, data_unitree);

    }


    void processTopicsInfo()
    {
        ros::Rate rate(5000);
        while (ros::ok())
        {
            rate.sleep();
        }
    }

};


int main(int argc, char** argv)
{ 
    ros::init(argc, argv, "MsgProcess");

    A1Msg2jointStateMsg MO;

    ROS_INFO("\033[1;32m----> unitreeMsg2jointStateMsg Procee Started.\033[0m");

    std::thread MsgProcessThread(&A1Msg2jointStateMsg::processTopicsInfo, &MO);

    ros::spin();

    MsgProcessThread.join();

    return 0;
}